
#include <util/types.h>
#include <util/common_macros.h>
#include <util/math_macros.h>
#include <util/mem.h>
#include <util/msg.h>

#include "../image.h"
#include "../pointop/threshold.h"
#include "xcorner.h"

// =============================================================================

static int __row_stride = 0;
static int __fast_ring_16[16];

static void __init_ring_offsets(int stride) {

	int w;
	__row_stride = stride;
	w = __row_stride;

	__fast_ring_16[0] = -(w + w);
	__fast_ring_16[4] = +(2);
	__fast_ring_16[8] = +(w + w);
	__fast_ring_16[12] = -(2);

	__fast_ring_16[1] = __fast_ring_16[0] + 1;
	__fast_ring_16[2] = __fast_ring_16[1] + 1 + w;
	__fast_ring_16[3] = __fast_ring_16[4] - w;
	__fast_ring_16[5] = __fast_ring_16[4] + w;
	__fast_ring_16[6] = __fast_ring_16[5] - 1 + w;
	__fast_ring_16[7] = __fast_ring_16[8] + 1;
	__fast_ring_16[9] = __fast_ring_16[8] - 1;
	__fast_ring_16[10] = __fast_ring_16[9] - 1 - w;
	__fast_ring_16[11] = __fast_ring_16[12] + w;
	__fast_ring_16[13] = __fast_ring_16[12] - w;
	__fast_ring_16[14] = __fast_ring_16[13] + 1 - w;
	__fast_ring_16[15] = __fast_ring_16[0] - 1;

}

/**
 * 
 * @param Mat
 * @param x
 * @param y
 * @param gate
 * @return 
 */
static int __is_Xcorner(ImpMatrix * Mat, int x, int y, int gate) {

	int pixels[16];
	real * mat;
	int i;
	int im;
	int _i;
	int sum, mean;
	real score;

	/* avoid invalid indexing */
	if (x < 2 || x > Mat->columns - 3) return FALSE;
	if (y < 2 || y > Mat->rows - 3) return FALSE;

	mat = Mat->data;

	if (__row_stride != Mat->columns) __init_ring_offsets(Mat->columns);

	im = imp_matrix_idx(Mat, x, y);

	sum = 0.0;
	for (i = 0; i < 16; i++) {
		_i = im + __fast_ring_16[i];
		pixels[i] = mat[ _i ];
		sum += pixels[i];
	}

	mean = sum / 16.0;

	//gate = 20;

	int highTh = mean + gate;
	int lowTh = mean - gate;

	if (mat[im] <= lowTh || mat[im] >= highTh) return FALSE;

	boolean state = (pixels[15] > mean);
	int nswaps = 0;
	for (i = 0; i < 16; i++) {
		int val = pixels[i];

		if (state) {
			if (val < lowTh) {
				state = FALSE;
				nswaps++;
			}
		} else
			if (val > highTh) {
			state = TRUE;
			nswaps++;
		};
	}

	if (nswaps != 4) {
		/* Is not a X-corner */
		return 0;
	}

	/* Compute corner score */
	int sbright = 0;
	int sdark = 0;
	int ndark = 0;
	int nbright = 0;

	
	for (i = 0; i < 16; i++) {
		if (pixels[i] > mat[im]) {
			sbright += pixels[i] - mat[im];
			nbright++;
		} else {
			sdark += mat[im] - pixels[i];
			ndark++;
		}
	}
	
	score = MAX(sbright, sdark);
	//score = MIN(sbright, sdark);
	//score = 1 - ABS(nbright - ndark) / 16;
	
	return score;
}

int imp_image_features_Xcorner(ImpMatrix * Mat, ImpMatrix ** Corners) {

	int n = 0;
	int x_corner_gate = 10;

	real * corners;

	if (*Corners == NULL) (*Corners) = imp_matrix_new(Mat->rows, Mat->columns);
	corners = (*Corners)->data;

	int r, c, _r, i;
	int rows, columns;

	rows = (*Corners)->rows;
	columns = (*Corners)->columns;

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;
			corners[i] = __is_Xcorner(Mat, c, r, x_corner_gate);

			if (corners[i]) {
				n++;
			}
		}
	}
	return n;
}

// =============================================================================

void get_circle_idx_trace(int xm, int ym, int ray, int width, int ** vec, int * size) {

	int * idx;
	int sz = 0;
	int c = 0;
	int R = ray;

	int x = -ray, y = 0, err = 2 - 2 * ray; /* II. Quadrant */
	do {
		(sz)++;
		c++;
		ray = err;
		if (ray > x) err += ++x * 2 + 1; /* e_xy+e_x > 0 */
		if (ray <= y) err += ++y * 2 + 1; /* e_xy+e_y < 0 */
	} while (x < 0);

	(*size) = sz * 4;

	idx = mem_allocn((*size), int);
	c = 0;

	ray = R;
	x = -ray;
	y = 0;
	err = 2 - 2 * ray;

	//sz = sz - 1;

	do {

		idx[c + sz * 0] = (xm - x) + (ym + y) * width; // 0
		idx[c + sz * 1] = (xm - y) + (ym - x) * width; // - 90
		idx[c + sz * 2] = (xm + x) + (ym - y) * width; // 180
		idx[c + sz * 3] = (xm + y) + (ym + x) * width; // 90

		c++;

		ray = err;
		if (ray > x) err += ++x * 2 + 1; /* e_xy+e_x > 0 */
		if (ray <= y) err += ++y * 2 + 1; /* e_xy+e_y < 0 */
	} while (x < 0);

	(*vec) = idx;
}

static real __is_Xcorner_N(ImpMatrix * Mat, int x, int y, int gate, int * __ring, int sz) {

	int pixels[sz];
	real * mat;
	int i;
	int im;
	int _i;
	int sum, mean;
	real score;

	/* avoid invalid indexing */
	if (x < 2 || x > Mat->columns - 3) return FALSE;
	if (y < 2 || y > Mat->rows - 3) return FALSE;

	mat = Mat->data;

	if (__row_stride != Mat->columns) __init_ring_offsets(Mat->columns);

	im = imp_matrix_idx(Mat, x, y);

	sum = 0.0;
	for (i = 0; i < sz; i++) {
		_i = im + __ring[i];

		if (_i < 0 || _i >= Mat->size) continue;

		pixels[i] = mat[ _i ];
		sum += pixels[i];
	}

	mean = (real) sum / (real) sz;

	//gate = 20;

	int highTh = mean + gate;
	int lowTh = mean - gate;

	if (mat[im] <= lowTh || mat[im] >= highTh) return FALSE;

	boolean state = (pixels[sz - 1] > mean);
	int nswaps = 0;
	for (i = 0; i < sz; i++) {
		int val = pixels[i];

		if (state) {
			if (val < lowTh) {
				state = FALSE;
				nswaps++;
			}
		} else
			if (val > highTh) {
			state = TRUE;
			nswaps++;
		};
	}

	if (nswaps != 4) {
		/* Is not a X-corner */
		return 0;
	}

	/* Compute corner score */
	int sbright = 0;
	int sdark = 0;

	int ndark = 0;
	int nbright = 0;

	for (i = 0; i < sz; i++) {
		if (pixels[i] > mat[im]) {
			sbright += pixels[i] - mat[im];
			nbright++;
		} else {
			sdark += mat[im] - pixels[i];
			ndark++;
		}
	}
	//score = MIN(sbright, sdark);
	//score = MAX(sbright, sdark);
	//score = 1.0 - ABS(ndark-nbright)/(ndark+nbright);

	/*
		score = nbright*sbright - ndark*sdark;
		score = score / ((nbright + ndark)*255.0);
		score = 1 - ABS(score);
	 */

	score = 1 - ABS(nbright - ndark) / sz;

	return score;
}

int imp_image_features_XcornerN(ImpMatrix * Mat, ImpMatrix ** Corners, int ray) {

	int count = 0;
	int x_corner_gate = 20;

	real * corners;

	if (*Corners == NULL) (*Corners) = imp_matrix_new(Mat->rows, Mat->columns);
	corners = (*Corners)->data;

	int r, c, _r, i;
	int rows, columns;

	int * __ring;
	int sz;

	get_circle_idx_trace(0, 0, ray, Mat->columns, &__ring, &sz);

	rows = (*Corners)->rows;
	columns = (*Corners)->columns;

	for (r = ray, _r = r * columns; r < rows - ray; r++, _r += columns) {
		for (c = ray; c < columns - ray; c++) {
			i = _r + c;
			corners[i] = __is_Xcorner_N(Mat, c, r, x_corner_gate, __ring, sz);

			if (corners[i]) count++;
		}
	}

	mem_free(__ring);

	return count;
}

void imp_image_features_Xcorners_subpixel(ImpMatrix * Mat, ImpMatrix ** Corners, ImpMatrix ** SubPixel) {

	ImpMatrix * Dx = NULL;
	ImpMatrix * Dy = NULL;
	ImpMatrix * Dxy = NULL;
	ImpMatrix * Dxx = NULL;
	ImpMatrix * Dyy = NULL;

	ImpMatrix * Hresp = NULL;
	ImpMatrix * NMS = NULL;

	ImpMatrix * MaskDil = NULL;

	real rx, ry, rxy, rxx, ryy;
	real den, s, t;

	ImpMatrix * SE = imp_matrix_new_ones(3, 3);

	//imp_image_save_matrix_as_image( Mat, TRUE, "Mat.pgm");

	// Get x-corners candidates
	imp_image_features_Xcorner(Mat, Corners);
	//imp_image_features_XcornerN(Mat, Corners, 3);

	//imp_image_save_matrix_as_image( *Corners, TRUE, "Corners.pgm");

	// Create a Mask for find sub-pixel x-corners
	imp_image_threshold(*Corners, 0, 0, 255, IMP_THRESHOLD_BINARY, NULL);

	//imp_image_save_matrix_as_image( *Corners, TRUE, "CornersBW.pgm");

	// Increate size of Mask
	imp_image_morph_dilate(*Corners, SE, &MaskDil);

	//imp_image_save_matrix_as_image( MaskDil, TRUE, "MaskDil.pgm");

	// Get the derivatives (Jacobian and Hessian Matrices )
	imp_matrix_derivative_with_mask(Mat, MaskDil, &Dx, &Dy, &Dxy);

	//imp_image_save_matrix_as_image( Dx, TRUE, "Dx.pgm");
	//imp_image_save_matrix_as_image( Dy, TRUE, "Dy.pgm");
	//imp_image_save_matrix_as_image( Dxy, TRUE, "Dxy.pgm");

	imp_matrix_derivative2_with_mask(Mat, MaskDil, &Dxx, &Dyy);

	//imp_image_save_matrix_as_image( Dxx, TRUE, "Dxx.pgm");
	//imp_image_save_matrix_as_image( Dyy, TRUE, "Dyy.pgm");

	Hresp = imp_matrix_new(Mat->rows, Mat->columns);

	// Calculates the x-corner response using the Hessian matrix
	int i;
	for (i = 0; i < Mat->size; i++) {

		rxx = Dxx->data[i];
		ryy = Dyy->data[i];
		rxy = Dxy->data[i];

		Hresp->data[i] = (rxx * ryy) - (rxy * rxy);

		Hresp->data[i] = ABS(Hresp->data[i]);
	}

	//imp_image_save_matrix_as_image( Hresp, TRUE, "Hresp.pgm");

	// Non-maximum suppression of Hresp
	imp_image_nonmaxsup_feature(Hresp, 4, MaskDil, &NMS);

	//imp_image_save_matrix_as_image( NMS, TRUE, "NMS.pgm");

	// Get the number of x-corners
	int ct = 0;
	for (i = 0; i < Mat->size; i++) {
		if (NMS->data[i] == 0) continue;
		ct++;
	}

	// Alloc 
	if (ct == 0) {
		*SubPixel = NULL;

		imp_object_delete(Dx);
		imp_object_delete(Dy);
		imp_object_delete(Dxy);
		imp_object_delete(Dxx);
		imp_object_delete(Dyy);
		imp_object_delete(Hresp);
		imp_object_delete(NMS);
		imp_object_delete(MaskDil);

		imp_object_delete(SE);

		return;
	}
	(*SubPixel) = imp_matrix_new(ct, 2);

	// Calculates the sub-pixel x-corners
	ct = 0;
	int r, c, _r;
	int rows = Mat->rows;
	int columns = Mat->columns;

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			if (NMS->data[i] == 0) continue;

			rx = Dx->data[i];
			ry = Dy->data[i];
			rxy = Dxy->data[i];
			rxx = Dxx->data[i];
			ryy = Dyy->data[i];

			den = (rxx * ryy - rxy * rxy);

			s = (ry * rxy - rx * ryy) / den;
			t = (rx * rxy - ry * rxx) / den;

			(*SubPixel)->data[ct] = c + s;
			(*SubPixel)->data[ct + 1] = r + t;

			ct += 2;
		}
	}

	imp_object_delete(Dx);
	imp_object_delete(Dy);
	imp_object_delete(Dxy);
	imp_object_delete(Dxx);
	imp_object_delete(Dyy);
	imp_object_delete(Hresp);
	imp_object_delete(NMS);
	imp_object_delete(MaskDil);

	imp_object_delete(SE);
}

// =============================================================================
